;; mostly copied from project-dsc/present_motion, thanks to Tsuda!

(defun pr2-present-motion ()
  (send *robot* :rarm :inverse-kinematics (make-coords :pos (float-vector 600 -200 1000)))
  (send *robot* :head :look-at #f(939.1546 13.9156 1036.729))
  ;;  (send *pr2* :head :look-at (send *pr2* :rarm :end-coords :worldpos))
  (send *ri* :angle-vector (send *robot* :angle-vector))
  (send *ri* :wait-interpolation))

(defun pr2-grasp-choco-pose ()
  (let* ((current-arm (check-tuckarm-pose :thre 50)) ;; nil rarm larm
         (free-arm (or current-arm :larm))
		 (msec 2000)
         (av-lst (list
		  (float-vector -50 0 -150 -80 0 0 0)
		  (float-vector -117.421 -20 -220 -110 -3.75561 -50 149.611)
		  (float-vector -117.421 -1.85502 -214.683 -121.53 -3.75561 -11.8644 149.611))))

    (send *robot* :larm :angle-vector #f(25 0 0 -130 0 0 0))
    (send *robot* :rarm :angle-vector #f(-25 0 0 -130 0 0 0))
    (send *robot* :torso :waist-z :joint-angle 51.4717)
    (send *ri* :angle-vector (send *robot* :angle-vector) msec)
    (send *ri* :wait-interpolation)

    (send *ri* :stop-grasp :rarm)
    (dolist (av av-lst)
      (send *robot* :rarm :angle-vector av)
      (send *ri* :angle-vector (send *robot* :angle-vector) msec)
      (send *ri* :wait-interpolation))

    (send *ri* :start-grasp :rarm)
    (dolist (av (reverse av-lst))
      (send *robot* :rarm :angle-vector av)
      (send *ri* :angle-vector (send *robot* :angle-vector) msec)
      (send *ri* :wait-interpolation)))
  )

(defun pr2-release-choco ()
  (speak-jp "はっぴいいばれんたいん")
  (unix:sleep 1)
  (send *ri* :stop-grasp :rarm)
  (unix:sleep 5)
  (pr2-tuckarm-pose :rarm)
  (send *ri* :start-grasp :rarm)
  )

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun hrp2-grasp-present (&key (real nil) (debug nil) )
  ;; prepare hand
  (send *robot* :larm :hand :angle-vector *open-av*)
  ;;(send *pickview* :draw-objects)
  (when debug (read-line))
  (if real (send-angle-vector-hands))
  ;;
  (let ((av-lst
         (list 
          (float-vector 19.3206 59.8536 -37.7516 -59.211 6.3168 -26.589 -85.1144 -15.0) ;;pre motion
          (float-vector 19.6222 51.2565 -38.8339 -82.865 6.77353 -21.6091 -56.7527 -15.0) ;;grasp motion
          (float-vector 19.3206 59.8536 -37.7516 -59.211 6.3168 -26.589 -85.1144 -15.0) ;;post motion
          (float-vector -32.1858 25.7893 -26.9749 -52.4182 29.7107 -25.5043 -14.0853 -60.0) ;; present motion
         ))
        (cntr 0))
    (send *robot* :reset-manip-pose)
    ;;(send *pickview* :draw-objects)
    (when debug (read-line))
    (when real
      (send *ri* :angle-vector (send *robot* :angle-vector) 10000 *wor*)
      (send *ri* :wait-interpolation))
    (dolist (l av-lst)
      (send *robot* :larm :angle-vector l)
      ;;(send *pickview* :draw-objects)
      (when debug (read-line))
      (when real
        (send *ri* :angle-vector (send *robot* :angle-vector) 10000 *wor*)
        (send *ri* :wait-interpolation))
      (when (= cntr 1)
        (send *robot* :larm :hand :thick-pinch)
        ;;(send *pickview* :draw-objects)
        (when debug (read-line))
        (if real (send-angle-vector-hands)))
      (incf cntr))
    (send *robot* :head :look-at
	  (send (send (send *robot* :larm :end-coords :copy-worldcoords)
		      :translate (float-vector 100 0 0)) :worldpos))
    ))

(defun hrp2-release-choco (&key (real nil) (debug nil))
    (send *robot* :larm :hand :angle-vector *open-av*)
    ;;(send *pickview* :draw-objects)
    (when debug (read-line))
    (if real (send-angle-vector-hands))
    (send *robot* :reset-manip-pose)
    ;;(send *pickview* :draw-objects)
    (when debug (read-line))
    (when real
      (send *ri* :angle-vector (send *robot* :angle-vector) 10000 *wor*)
      (send *ri* :wait-interpolation))
    )

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(load "package://pr2eus/pr2-interface.l");;;
(defmethod corbaif
  (:go-pos-unsafe
   (x y &optional (d 0)) ;; [m] [m] [degree]
   (let (msec step goal (maxvel 0.3) (maxrad 0.5))
     ;; package://pr2_base_trajectory_action/config/pr2_base_link.yaml
     ;; 80% of maxvel = 0.3[m/sec]
     ;; 80% of maxrad = 0.5[rad/sec]
     (setq msec (* 1000 (max (/ (norm (float-vector x y)) (* maxvel 0.8))
			     (/ (abs (deg2rad d)) (* maxrad 0.8)))))
     (setq step (/ 1000.0 msec))
     (setq goal (send self :move-trajectory (* x step) (* y step) (* (deg2rad d) step) msec :stop t))
     (send move-base-trajectory-action :send-goal goal)
     (send move-base-trajectory-action :wait-for-result)
     ))
  (:move-trajectory
   (x y d &optional (msec 1000) &key (stop t)) ;; [m/sec] [m/sec] [rad/sec]
   (ros::spin-once)
   (let ((sec (/ msec 1000.0))
	 (odom-pos (scale 0.001 (send (send self :state :odom :pose) :pos)))
	 (odom-angle (elt (car (send (send self :state :odom :pose) :rpy-angle)) 0))
	 (msg (instance trajectory_msgs::JointTrajectory :init))
	 (goal (instance pr2_controllers_msgs::JointTrajectoryActionGoal :init))
	 (pt1 (instance trajectory_msgs::JointTrajectoryPoint :init))
	 (pt2 (instance trajectory_msgs::JointTrajectoryPoint :init)))
     (send pt1 :time_from_start (ros::time))
     (send pt2 :time_from_start (ros::time sec))
     (send pt1 :positions (v+ odom-pos
			      (float-vector 0 0 odom-angle)))
     (send pt2 :positions (v+ (v+ odom-pos
				  (float-vector 0 0 (+ odom-angle (* sec d))))
			     (rotate-vector (scale sec (float-vector x y 0))
					    odom-angle :z)))
     (send pt1 :velocities (rotate-vector (float-vector x y d) odom-angle :z))
     (if stop
	 (send pt2 :velocities (float-vector 0 0 0)) ;; To stop just
       (send pt2 :velocities (rotate-vector (float-vector x y d) odom-angle :z)))
     ;;
     (send msg :header :stamp (ros::time-now))
     (send msg :joint_names (list "base_link_x" "base_link_y" "base_link_pan"))
	 (send msg :points (list pt1 pt2))
     (send goal :goal :trajectory msg)
     ;;
     goal))
  )